#include "obstacle_detector/convexhull_display.h"
#include <pangolin/pangolin.h>

namespace obstacle_detector
{

ConvexHullDisplay::ConvexHullDisplay()
{
	debug_points_=new int16_t [200000*4];
	debug_point_num_=0;

	extreme_points_=new uint16_t [200000];
	extreme_points_num_=0;

	debug_points_float_=new float [200000*4];
	debug_points_num_float_=0;

	thread_ = std::thread(std::bind(&ConvexHullDisplay::debug_display,
		this));
}

ConvexHullDisplay::~ConvexHullDisplay()
{
	delete [] debug_points_;
}

void ConvexHullDisplay::set_showinfo(const int16_t* points, int point_num)
{
	data_mutex_.lock();
	memcpy(debug_points_,points,sizeof(int16_t)*point_num*4);
	debug_point_num_=point_num;
	data_mutex_.unlock();
}

void ConvexHullDisplay::set_showinfo(const float* points, int point_num)
{
	data_mutex_.lock();
	memcpy(debug_points_float_,points,sizeof(float)*point_num*4);
	debug_points_num_float_=point_num;
	data_mutex_.unlock();
}

void ConvexHullDisplay::set_extreme_points(const uint16_t *extreme_points, int point_num)
{
	memcpy(extreme_points_,extreme_points,sizeof(uint16_t)*point_num*33);
	extreme_points_num_=point_num;
}

void ConvexHullDisplay::set_bbox(const float *bbox, int bbox_num,
	const module_base::Location &loc)
{

	Eigen::Vector2f loc_diff(loc.x,loc.y);
	Eigen::Matrix2f loc_rotation;
	loc_rotation << cos(loc.yaw),sin(loc.yaw),
				 -1*sin(loc.yaw),cos(loc.yaw);

	std::vector<Eigen::MatrixXf> all_bboxs;
	for (int i=0;i<bbox_num;i++)
	{
		// printf ("label bbox debug %d: %f,%f,%f,%f\n",i,bbox[i*4+0],
		// 	bbox[i*4+1],bbox[i*4+2],bbox[i*4+3]);
		float yaw=-1*bbox[i*6+1];
		if (yaw==0)
		{
			continue;
		}
		// printf ("label %d: %d\n",i,all_bboxs.size());
		Eigen::MatrixXf single_bbox(2,4);
		single_bbox(0,0)=bbox[i*6+3];
		single_bbox(1,0)=bbox[i*6+5];
		single_bbox(0,1)=bbox[i*6+3];
		single_bbox(1,1)=bbox[i*6+4];
		single_bbox(0,2)=bbox[i*6+2];
		single_bbox(1,2)=bbox[i*6+4];
		single_bbox(0,3)=bbox[i*6+2];
		single_bbox(1,3)=bbox[i*6+5];
		Eigen::Matrix2f rotation;
		rotation << cos(yaw),sin(yaw),
				 -1*sin(yaw),cos(yaw);

		auto final_bbox=loc_rotation*((rotation*single_bbox).colwise()-loc_diff);
		all_bboxs.push_back(final_bbox);
	}
	// printf ("all_bboxs size: %d\n",all_bboxs.size());
	debug_bbox_=all_bboxs;
}

void ConvexHullDisplay::debug_display()
{
	pangolin::CreateWindowAndBind("convex_hull",640,480);
	glEnable(GL_DEPTH_TEST);

	pangolin::OpenGlRenderState s_cam(
		pangolin::ProjectionMatrix(640,480,420,420,320,240,0.2,1000),
		pangolin::ModelViewLookAt(0,-50,10, 0,0,10, pangolin::AxisZ)
	);

	pangolin::Handler3D handler(s_cam);
	pangolin::View& d_cam = pangolin::CreateDisplay()
		.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0f/480.0f)
		.SetHandler(&handler);


	while( !pangolin::ShouldQuit())
	{
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
		d_cam.Activate(s_cam);

		glPointSize(3);
		glBegin(GL_POINTS);
		glColor3f(0,1,0);
		data_mutex_.lock();
		for (int i=0;i<debug_point_num_;i++)
		{
			glVertex3f(debug_points_[i*4+0]*1.0/256,debug_points_[i*4+1]*1.0/256,0);
		}

		for (int i=0;i<debug_points_num_float_;i++)
		{
			glVertex3f(debug_points_float_[i*4+0],debug_points_float_[i*4+1],0);
		}


		glEnd();

		glPointSize(10);
		glBegin(GL_POINTS);
		for (int i=0;i<extreme_points_num_;i++)
		{
			if (extreme_points_[i*33+0]==0)
			{
				continue;
			}

			for (int j=0;j<16;j++)
			{
				uint16_t x=extreme_points_[i*33+j*2+1];
				uint16_t y=extreme_points_[i*33+j*2+2];
				if (x==0 && y==0)
				{
					continue;
				}
				float x_float=(x-32768)*1.0/256;
				float y_float=(y-32768)*1.0/256;
				if (j==0)
				{
					glColor3f(1,1,0);
				}
				else
				{
					glColor3f(1,0,0);
				}
				glVertex3f((x-32768)*1.0/256,(y-32768)*1.0/256,0);
			}
		}
		data_mutex_.unlock();

		glEnd();

		glLineWidth(1);
		glBegin ( GL_LINES );
		glColor3f(1,0,0);
		glVertex3f(0,0,0);
		glVertex3f(0,0,10);
		glColor3f(0,0,1);
		glVertex3f(0,0,0);
		glVertex3f(10,0,0);
		glColor3f(0,1,0);
		glVertex3f(0,0,0);
		glVertex3f(0,10,0);

		glColor3f(1,0,0);
		for (int i=0;i<extreme_points_num_;i++)
		{
			if (extreme_points_[i*33+0]==0)
			{
				continue;
			}

			for (int j=0;j<16;j++)
			{
				uint16_t x=extreme_points_[i*33+j*2+1];
				uint16_t y=extreme_points_[i*33+j*2+2];
				if (x==0 && y==0)
				{
					continue;
				}
				uint16_t x_next;
				uint16_t y_next;
				for (int m=j+1;m<17;m++)
				{
					int next_index=m%16;
					x_next=extreme_points_[i*33+next_index*2+1];
					y_next=extreme_points_[i*33+next_index*2+2];
					if (x_next==0 && y_next==0)
					{
						continue;
					}
					break;
				}
				glVertex3f((x-32768)*1.0/256,(y-32768)*1.0/256,0);
				glVertex3f((x_next-32768)*1.0/256,(y_next-32768)*1.0/256,0);
			}
		}


		glColor3f(0,1,1);
		for (int i=0;i<debug_bbox_.size();i++)
		{
			for (int j=0;j<4;j++)
			{
				glVertex3f(debug_bbox_[i](0,j),debug_bbox_[i](1,j),0);
				glVertex3f(debug_bbox_[i](0,(j+1)%4),debug_bbox_[i](1,(j+1)%4),0);
			}
		}


		glEnd();

		pangolin::FinishFrame();
	}
	pangolin::DestroyWindow("motion_obstacle");
}

}